/**
 * WTF Engine
 *
 * Released under the terms of the Worcester Public License,
 * see LICENSE in the root folder. This program comes with
 * no warranty whatsoever, implied or otherwise. All rights
 * other than those explicitly specified in the LICENSE document
 * are reserved.
 **
 * Collision Detection
 **
 * Author: Sean Micklethwaite
 **/

#define NDEBUG

#include "physics.h"
#include "aux-math.h"
#ifdef __PHYSICS_INTERNAL__

using namespace WtfEngine;

#define TIME_INFINITY			1000000
#define THRESHOLD_TIME			0.001
#define CONTACT_THRESHOLD		0.1
#define COLLISION_THRESHOLD		(-0.0001)


/**** Definitions ****/

bool __TrianglePointIntersection(const Triangle& tri, const tPosition& p)
{
	tVector		u, v, w;
	tScalar		uu, uv, vv, wu, wv, D, a, b;
	
	//
	// Check that point
	// lies within the triangle by calculating the point's barycentric
	// coordinates
	//

	u = tri[1] - tri[0];
	v = tri[2] - tri[0];
	w = p - tri[0];

	uu = u.dot(u);
	uv = u.dot(v);
	vv = v.dot(v);
	wu = w.dot(u);
	wv = w.dot(v);
	D = (uv * uv) - (uu * vv);

	// Test parametric coords
	a = ((uv * wv) - (vv * wu)) / D;
	if((a < 0.0) || (a > 1.0))
		return false;
	b = ((uv * wu) - (uu * wv)) / D;
	if ((b < 0.0) || ((a + b) > 1.0))
		return false;
	
	return true;
};

inline static bool __TriangleRayIntersection(const Triangle& tri, const tDirection& norm,
											const tPosition& disp, const tPosition& vel,
							 tPosition& p, tScalar& t) {
	// Intersection time
	t = norm.dot(disp - tri[0]) / norm.dot(vel);
	
	if((0 < t) && (t <= 1.0)) {
		//
		// Intersection occurs, check that point
		// lies within the triangle by calculating the point's barycentric
		// coordinates
		//

		p = (disp + (vel * t));
		
		return (__TrianglePointIntersection(tri, p));
	} else {
		return false;
	};
};

bool __TriangleIntersection(const Triangle& t1, const Triangle& t2,
							const tDirection& n1, const tDirection& n2,
						    tPosition& point, tDirection& norm)
{
	int i;
	tScalar d1 = n1.dot(t1[0]), d2 = n2.dot(t2[0]);
	tVector r1, r2;
	
	// Calculate distances of vertices from opposing plane
	for(i = 0; i < 3; i++)
		r2[i] = n1.dot(t2[i]) + d1;
	for(i = 0; i < 3; i++)
		r1[i] = n2.dot(t1[i]) + d2;
	
	if(r2 > 0 || r1 > 0 || r2 < 0 || r1 < 0)
		return false;
	else if(r1 == 0 || r2 == 0) {
		// Coplanar triangles
		// Test each point
		
	} else {
		// There exists a line of intersection between the planes.
		// Project the vertices of each triangle onto the line.
		
		tDirection l(n1.cross(n2).normalize());
		tVector p1, p2;
		tVector s1, s2; // Line parameters
		
		for(i = 0; i < 3; i++)
			p2[i] = l.dot(t2[i]);
		for(i = 0; i < 3; i++)
			p1[i] = l.dot(t1[i]);
		
		// Compute line parameters
		s1[0] = p1[1] + ((p1[2] - p1[1]) * (r1[1] / (r1[1] - r1[2])));
		s1[1] = p1[0] + ((p1[2] - p1[0]) * (r1[0] / (r1[0] - r1[2])));
		s1[2] = p1[1] + ((p1[0] - p1[1]) * (r1[1] / (r1[1] - r1[0])));
		
		s2[0] = p2[1] + ((p2[2] - p2[1]) * (r2[1] / (r2[1] - r2[2])));
		s2[1] = p2[0] + ((p2[2] - p2[0]) * (r2[0] / (r2[0] - r2[2])));
		s2[2] = p2[1] + ((p2[0] - p2[1]) * (r2[1] / (r2[1] - r2[0])));
		
		for(i = 0; i < 3; i++) {
			s1[i] = l.dot(t1[i]);
		};
	};
};


/**
 * Checks bounding spheres for intersection, taking into account
 * relative velocities between the objects
 **/
bool PhysicalModel::CheckBounds(const PhysicalModel& obj,
						 const tVector& disp,
						 const tVector& vel) const
{
	return (disp.length() <= (disp.normalize().dot(vel)
		+ mBoundingRadius + obj.mBoundingRadius));
};


bool PhysicalModel::CheckCollision(const WtfGraphics::Mesh& obj,
			const tVector& disp, // Displacement of obj, to our origin
			const tVector& meshVel, // Mesh velocity
			const tOrientation& meshOri, // Mesh orientation
			tVector& point, tVector& normal, tVector& relVel, tScalar& time) const
{
	int			i, j, n = 0, tot = 0, intersections = 0;
	tScalar		d, t;
	tVector		norm, p, vel;
	Triangle	tri;

	time = TIME_INFINITY;

	for(i = 0; i < obj.getNumTris(); i++)
	{
		norm = meshOri * obj.getFaceNormal(i);

		//
		// Reorient the mesh triangle
		//

		tri[0] = meshOri * obj[i][0];
		tri[1] = meshOri * obj[i][1];
		tri[2] = meshOri * obj[i][2];

		for(j = 0; j < mNumVertices; j++)
		{
			vel = mzNextVertex[j] - (mzVertex[j] + meshVel);
			if(__TriangleRayIntersection(tri, norm, mzVertex[j] - disp, vel, p, t)) {
				// Make sure all our normals point the same way
				if(norm.dot(disp) > 0) norm = -norm;

				if(abs(t - time) <= THRESHOLD_TIME){
					// Simultaneous collision - take averages
					point = ((point * n) + mzLocalVertex[j]) / (n + 1);
					normal = ((normal * n) + norm) / (n + 1);
					relVel = ((relVel * n) + vel) / (n + 1);
					n++;
				}else{
					// New earliest collision
					point = mzLocalVertex[j];
					normal = norm;
					relVel = vel;
					time = t;
					n = 1;
				};
				tot++;
			};
		};
	};

#ifndef NDEBUG
	if((n > 0) || (intersections > 0))
		std::clog << n << " collision point(s), " << tot << "/" << intersections
			<< " considered, " << mNumVertices * obj.getNumTris() << " tested" << std::endl;
#endif

	time = maxf(0, time - THRESHOLD_TIME);
	return (n > 0);
};

bool Physical::CheckCollision(WtfEngine::Physical &obj,
							   tVector& point,
							   tVector& normal,
							   tVector& relVel,
							   tScalar& time)
{
	//
	// Perform bounds check
	//

	if(!this->mModel->CheckBounds(*(obj.mModel),
		obj.currentPosition - this->currentPosition,
		obj.currentVelocity - this->currentVelocity)) return false;


	//
	// Perform advanced collision check
	//

	this->mModel->setVertexArrays(mzVertex, mzNextVertex);
	if(!this->mModel->CheckCollision(obj.getMesh(),
		getOrientation().transpose() * (obj.currentPosition - this->currentPosition),
		getOrientation().transpose() * obj.currentVelocity * time,
		obj.currentOrientation * getOrientation().transpose(),
		point, normal, relVel, time)) return false;

	return true;
};

bool Static::CheckCollision(WtfEngine::Physical &obj,
							   tVector& point,
							   tVector& normal,
							   tVector& relVel,
							   tScalar& time)
{
	//
	// Perform advanced collision check
	//

	obj.mModel->setVertexArrays(obj.mzVertex, obj.mzNextVertex);
	bool c = obj.getModel().CheckCollision(this->getMesh(),
		obj.getOrientation().transpose() * (this->currentPosition - obj.getPosition()), tVector(),
		// The mesh is oriented to the world, but needs to be oriented to model-local
		// coordinates. 
		this->currentOrientation * obj.getOrientation().transpose(),
		point, normal, relVel, time);

	return c;
};

void Physical::DoCollisionDetection(CollisionHeap& heap)
{
	PhysicsEngine::tPassiveList::iterator	j;
	PhysicsEngine::tStaticList::iterator	k;

	tVector point, normal, relVel;
	tScalar time, dt;

	//
	// Update object's model based upon new orientation & velocities
	//

	dt = Kernel::GetInstance().GetGameState()->GetDelta() - mLocalTime;

	this->mModel->UpdateModel(currentOrientation, currentVelocity,
		mAngularVelocity, dt, mzVertex, mzNextVertex);

	//
	// Check with passive objects
	// TODO: octree optimisation
	//

	for(j = PhysicsEngine::GetInstance().GetPassive().begin();
		j != PhysicsEngine::GetInstance().GetPassive().end();
		++j)
	{
		if(this != &**j) {
			if((**j).isParticle)
				continue;

			// Update object to local time frame
			if((*j)->mLocalTime != mLocalTime)
				(*j)->UpdatePhysical(mLocalTime);

			time = dt;
			if(this->CheckCollision(**j, point, normal, relVel, time)) {
				
				//
				// Collision has occured - fire event
				//
				
				std::cout << "Collision with passive\n";
				time *= dt;
				relVel = relVel / dt;
				heap.Insert(Collision(*this, **j,
						relVel, point, normal, time), time);
			};
		};
	};


	//
	// Check with static objects
	//

	for(k = PhysicsEngine::GetInstance().GetStatic().begin();
		k != PhysicsEngine::GetInstance().GetStatic().end();
		++k)
	{
		if((*k)->CheckCollision(*this, point, normal, relVel, time)) {

			//
			// Collision has occured - add into heap
			//

#			ifndef NDEBUG
				std::cout << "Collision with static at " 
				<< point[0] << ", " << point[1] << ", " << point[2] << "; normal "
				<< normal[0] << ", " << normal[1] << ", " << normal[2] << "\n";
#			endif

			// Scale time and velocity to unit values
			time *= dt;
			relVel = relVel / dt;
			heap.Insert(Collision(*this, **k,
						relVel, point, normal, time), time);
		};
	};
};


/**
 * The collision detection task
 **/
void CollisionDetection::Run()
{
	PhysicsEngine::tPassiveList::iterator	p;
	PhysicsEngine::tActiveList::iterator	i;

	// This priority queue allows us to process collisions in order
	// of time
	CollisionHeap	heap;

	//
	// Update forces
	//
	
	for(p = PhysicsEngine::GetInstance().GetPassive().begin();
		   p != PhysicsEngine::GetInstance().GetPassive().end();
		   ++p)
	{
		(*p)->UpdateForces();
	};

	PhysicalSystem::Update();
	
	
	//
	// First detection pass
	// Process all active objects.
	// TODO: This can be parallelized.
	//

	for(i = PhysicsEngine::GetInstance().GetActive().begin();
		i != PhysicsEngine::GetInstance().GetActive().end();
		++i)
	{
		(*i)->DoCollisionDetection(heap);
	};


	//
	// Response loop:
	// Process all collisions in order of time. Note that
	// new collisions may enter the heap after processing
	// an earlier collision, hence multiple collisions per
	// object per frame can be handled seamlessly.
	//

	while(!heap.IsEmpty()){
		heap.PopMin().processCollision(heap);
	};
};

void Collision::processCollision(CollisionHeap& heap)
{
	//
	// Update object position to collision point, remove any
	// other collisions relating to this object from the heap.
	//

	this->movingObject->UpdatePhysical(this->time);
	heap.RemoveObject(this->movingObject);
	
	if(this->physObject == NULL) {
		
		//
		// Fire collision event
		//
	
		Kernel::GetInstance().GetGameStatePtr()->ProcessEvent(
							new ObjectCollisionEvent(this->movingObject, this->staticObject));
		
		//
		// Moving object colliding with a static
		//

		if(!PhysicalSystem::Connect(movingObject, staticObject, point, normal,
			relativeVelocity, CONTACT_THRESHOLD, 0.15)){
			// Not connected - reapply collision detection
			movingObject->DoCollisionDetection(heap);
		};
		
	}else{
		
		//
		// Two moving objects colliding
		// Update position of collidee, and remove from heap
		//

		this->physObject->UpdatePhysical(this->time);
		heap.RemoveObject(this->physObject);


		//
		// Fire collision event
		//
	
		Kernel::GetInstance().GetGameStatePtr()->ProcessEvent(
							new ObjectCollisionEvent(this->movingObject, this->staticObject));
		
		if(!PhysicalSystem::Connect(movingObject, physObject, point, normal,
			relativeVelocity, CONTACT_THRESHOLD, 0.1)){

			//
			// Reapply collision detection to collidee
			//
				
			movingObject->DoCollisionDetection(heap);
			physObject->DoCollisionDetection(heap);
		};
	};
};

void CollisionHeap::RemoveObject(WtfEngine::Physical *pObject)
{
	int i;
	for(i = 0; i < this->GetSize(); i++){
		if(this->Get(i).relatesTo(pObject))
			this->Remove(i);
	};
};

#endif
